﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

namespace ConsoleApplication1
{
    class Juego
    {
        int Z_Alto;
        int X_Ancho;
        int Y_Profundidad;
        //static int tag;
        List<Bitmap> imagenesLaberinto;
        List<Robot> robotsList;
        Laberinto laberinto;
        List<int[]> movsFactiblesInverso;
        List<int[]> movsFactibles;
        //List<Robot> robotsJuego;

        
        
        //int numRobots;//para que algoritmo de busqueda pare cuando haya alcanzado este numero de robots
        //int numRobotsAlcanzados;
        int numRobotsUp;
        int numRobotsDown;
        int numRobotsLeft;
        int numRobotsRight;
        int numRobotsFront;
        int numRobotsBack;
        int numRobotsTotal;
        int numRobotsAlcanzados;
        int numRobotsAlcanzadosBack;
        int numRobotsAlcanzadosFront;
        int numRobotsAlcanzadosLeft;
        int numRobotsAlcanzadosRight;
        int numRobotsAlcanzadosUp;
        int numRobotsAlcanzadosDown;

        public Juego( List<Bitmap> imagenes, List<int[]> restPos, List<string> restMov)
        {
            robotsList = new List<Robot>();
            this.imagenesLaberinto = imagenes;
            this.imagenesLaberinto[0].Save("Prueba.bmp");
            //Cargo lista de movimientos en sentido opuesto a prioridad, pues se busca desde la salida a la posición del robot en el algoritmo de busqueda
            //Console.WriteLine("Entrando a constructor de juego");
            Z_Alto = imagenes.Count;  // Eje Z
            X_Ancho = imagenes[0].Width; // Eje X
            Y_Profundidad = imagenes[0].Height;//Eje Y
            movsFactiblesInverso = new List<int[]>();
            numRobotsRight = 0;
            numRobotsLeft = 0;
            numRobotsUp = 0;
            numRobotsDown = 0;
            numRobotsFront = 0;
            numRobotsBack = 0;
            numRobotsTotal = 0;

            // Orden de prioridad: Arriba Atras Derecha Frente Izquierda Abajo
            movsFactiblesInverso.Add(new int[] { 0, 0 ,1});//Abajo
            movsFactiblesInverso.Add(new int[] { -1, 0 ,0});//Izquierda
            movsFactiblesInverso.Add(new int[] { 0, 1 ,0});
            movsFactiblesInverso.Add(new int[] { 1, 0 ,0});
            movsFactiblesInverso.Add(new int[] { 0, -1, 0 });
            movsFactiblesInverso.Add(new int[] { 0, 0, -1 });
            //movsFactibles = new List<int[]>();
            //movsFactibles.Add(new int[] { 0, 0 ,-1});
            //movsFactibles.Add(new int[] { 0, -1 ,0});
            //movsFactibles.Add(new int[] { 1, 0 ,0});
            //movsFactibles.Add(new int[] { 0, 1 ,0});
            //movsFactibles.Add(new int[] { -1, 0, 0 });
            //movsFactibles.Add(new int[] { 0, 0, 1 });
            laberinto = new Laberinto(X_Ancho, Y_Profundidad, Z_Alto);
            //numRobots = numerRobots;
            //Console.WriteLine("Comenzando Definicion de salidas");
            #region Definición de Salidas (laberinto.GoalUp, laberinto.GoalDown, laberinto.GoalLeft, etc)
            if (Z_Alto % 2 == 0)// Para 2D --> Z_Alto = 1 por lo que nunca entra acá
            {
                if (Y_Profundidad % 2 == 0)
                {
                    laberinto.GoalLeft = laberinto.Terreno[0, Y_Profundidad/2, Z_Alto / 2];
                    laberinto.GoalRight = laberinto.Terreno[X_Ancho-1, Y_Profundidad/2, Z_Alto / 2];
                }
                else
                {
                    laberinto.GoalLeft = laberinto.Terreno[0, Y_Profundidad/2 - 1/2, Z_Alto / 2];
                    laberinto.GoalRight = laberinto.Terreno[X_Ancho - 1, Y_Profundidad / 2 - 1 / 2, Z_Alto / 2];
                }
                if (X_Ancho % 2 == 0)
                {
                    laberinto.GoalFront = laberinto.Terreno[X_Ancho / 2,Y_Profundidad-1, Z_Alto / 2];
                    laberinto.GoalBack = laberinto.Terreno[X_Ancho / 2, 0, Z_Alto / 2];
                }
                else
                {
                    laberinto.GoalFront = laberinto.Terreno[X_Ancho / 2-1, Y_Profundidad - 1, Z_Alto / 2];
                    laberinto.GoalBack = laberinto.Terreno[X_Ancho / 2 - 1, 0, Z_Alto / 2];
                }
            }

            else //if(Z_Alto % 2 == 1)
            {
                if (Y_Profundidad % 2 == 0)
                {
                    laberinto.GoalLeft = laberinto.Terreno[0, Y_Profundidad / 2, Z_Alto / 2-1/2];
                    laberinto.GoalRight = laberinto.Terreno[X_Ancho - 1, Y_Profundidad / 2, Z_Alto / 2-1/2];
                }
                else
                {
                    laberinto.GoalLeft = laberinto.Terreno[0, Y_Profundidad / 2 - 1 / 2, Z_Alto / 2-1/2];
                    laberinto.GoalRight = laberinto.Terreno[X_Ancho - 1, Y_Profundidad / 2 - 1 / 2, Z_Alto / 2-1/2];
                }
                if (X_Ancho % 2 == 0)
                {
                    laberinto.GoalFront = laberinto.Terreno[X_Ancho / 2, Y_Profundidad - 1, Z_Alto / 2-1/2];
                    laberinto.GoalBack = laberinto.Terreno[X_Ancho / 2, 0, Z_Alto / 2-1/2];
                }
                else
                {
                    laberinto.GoalFront = laberinto.Terreno[X_Ancho / 2 - 1, Y_Profundidad - 1, Z_Alto / 2-1/2];
                    laberinto.GoalBack = laberinto.Terreno[X_Ancho / 2 - 1, 0, Z_Alto / 2-1/2];
                }
            }
            if (imagenes.Count > 1)
            {
                if (X_Ancho % 2 == 0)
                {
                    if (Y_Profundidad % 2 == 0)
                    {
                        laberinto.GoalUp = laberinto.Terreno[X_Ancho / 2, Y_Profundidad / 2, 0];
                        laberinto.GoalDown = laberinto.Terreno[X_Ancho / 2, Y_Profundidad / 2, Z_Alto - 1];
                    }
                    else //if (Y_Profundidad % 2 == 1)
                    {
                        laberinto.GoalUp = laberinto.Terreno[X_Ancho / 2, Y_Profundidad / 2 - 1 / 2, 0];
                        laberinto.GoalDown = laberinto.Terreno[X_Ancho / 2, Y_Profundidad / 2 - 1 / 2, Z_Alto - 1];
                    }
                }
                else //if (X_Ancho % 2 == 1)
                {
                    if (Y_Profundidad % 2 == 0)
                    {
                        laberinto.GoalUp = laberinto.Terreno[X_Ancho / 2 - 1 / 2, Y_Profundidad / 2, 0];
                        laberinto.GoalDown = laberinto.Terreno[X_Ancho / 2 - 1 / 2, Y_Profundidad / 2, Z_Alto - 1];
                    }
                    else //if (Y_Profundidad % 2 == 1)
                    {
                        laberinto.GoalUp = laberinto.Terreno[X_Ancho / 2 - 1 / 2, Y_Profundidad / 2 - 1 / 2, 0];
                        laberinto.GoalDown = laberinto.Terreno[X_Ancho / 2 - 1 / 2, Y_Profundidad / 2 - 1 / 2, Z_Alto - 1];
                    }
                }
            }
            else
            {
                laberinto.GoalUp = null;
                laberinto.GoalDown = null;
            }
            #endregion
            //Console.WriteLine("Comenzando Carga de Robots desde imagenes");
            #region Carga de Robots desde imagenes
            //Cargar el numero de robots
            int sumaColores;
             for (int z = 0; z < Z_Alto; z++)
            {
                 for (int x = 0; x < X_Ancho; x++)
                {
                    for (int y = 0; y < Y_Profundidad; y++)
                    {
                        sumaColores = imagenes[z].GetPixel(x, y).R + imagenes[z].GetPixel(x, y).G + imagenes[z].GetPixel(x, y).B;
                        if (  sumaColores !=0) //Si hay algún robot
                        {
                            this.laberinto.Terreno[x,y,z].RobotInHere = true;
                            if (sumaColores > 690)
                            {
                                Robot rob = new RobU();//Respetando la mala convención
                                rob.Coordenadas = new int[] { x, y, z };
                                numRobotsBack++;
                                numRobotsTotal++;
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                                
                            }
                            else if(sumaColores > 621 )
                            {
                                Robot rob = new RobD(); //Respetando la mala convención
                                rob.Coordenadas = new int[] { x, y, z };
                                numRobotsFront++;
                                numRobotsTotal++;
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }
                            else if(sumaColores > 552)
                            {

                                Robot rob = new RobL();
                                rob.Coordenadas = new int[] { x, y, z };
                                numRobotsLeft++;
                                numRobotsTotal++;
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }

                            else if(sumaColores > 483)
                            {
                                Robot rob = new RobR();
                                rob.Coordenadas = new int[] { x, y, z };
                                numRobotsRight++;
                                numRobotsTotal++;
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }
                            else if(sumaColores > 414)
                            {
                                Robot rob = new RobT();
                                rob.Coordenadas = new int[] { x, y, z };
                                numRobotsRight++;
                                numRobotsLeft++;
                                numRobotsUp++;
                                numRobotsDown++;
                                numRobotsFront++;
                                numRobotsBack++;
                                numRobotsTotal++;
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }
                            else if(sumaColores > 345)
                            {

                                Robot rob = new RobC();
                                rob.Coordenadas = new int[] { x, y, z };
                                numRobotsRight++;
                                numRobotsLeft++;
                                numRobotsUp++;
                                numRobotsDown++;
                                numRobotsFront++;
                                numRobotsBack++;
                                numRobotsTotal++;
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }
                            else if(sumaColores > 276)
                            {
                                Robot rob = new RobS1();
                                rob.Coordenadas = new int[] { x, y, z };
                                numRobotsRight++;
                                numRobotsLeft++;
                                numRobotsUp++;
                                numRobotsDown++;
                                numRobotsFront++;
                                numRobotsBack++;
                                numRobotsTotal++;
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }
                            else if(sumaColores > 207)
                            {
                                numRobotsRight++;
                                numRobotsLeft++;
                                numRobotsUp++;
                                numRobotsDown++;
                                numRobotsFront++;
                                numRobotsBack++;
                                numRobotsTotal++;
                                Robot rob = new RobS3();
                                rob.Coordenadas = new int[] { x, y, z };
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }
                            else if (sumaColores > 138)
                            {
                                numRobotsRight++;
                                numRobotsLeft++;
                                numRobotsUp++;
                                numRobotsDown++;
                                numRobotsFront++;
                                numRobotsBack++;
                                numRobotsTotal++;
                                Robot rob = new RobO();
                                rob.Coordenadas = new int[] { x, y, z };
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }
                            else if (sumaColores > 69)
                            {
                                numRobotsRight++;
                                numRobotsLeft++;
                                numRobotsUp++;
                                numRobotsDown++;
                                numRobotsFront++;
                                numRobotsBack++;
                                numRobotsTotal++;
                                Robot rob = new RobB();
                                rob.Coordenadas = new int[] { x, y, z };

                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }
                            else if (sumaColores > 0)
                            {
                                numRobotsRight++;
                                numRobotsLeft++;
                                numRobotsUp++;
                                numRobotsDown++;
                                numRobotsFront++;
                                numRobotsBack++;
                                numRobotsTotal++;
                                Robot rob = new RobX();
                                rob.Coordenadas = new int[] { x, y, z };
                                robotsList.Add(rob);
                                laberinto.Terreno[x, y, z].RobotCelda = rob;
                            }
                        }
                    }
                }
            }
            #endregion
            // Console.WriteLine("Comenzando CARGA DE PAREDES");
            #region CargarParedes
            //cargar las paredes
            for (int i = 0; i < restMov.Count; i++)
            {
                /*restrMovsInverso.Add(moverDown);
            restrMovsInverso.Add(moverLeft);
            restrMovsInverso.Add(moverFront);
            restrMovsInverso.Add(moverRight);
            restrMovsInverso.Add(moverBack);
            restrMovsInverso.Add(moverUp);*/
                int[] temp = restPos[i];
                if (restMov[i] == "L")
                {
                    laberinto.Terreno[temp[0], temp[1], temp[2]].RestrMovsInverso[1] = false;
                }
                else if (restMov[i] == "R")
                {
                    laberinto.Terreno[temp[0], temp[1], temp[2]].RestrMovsInverso[3] = false;
                }
                else if (restMov[i] == "F")
                {
                    laberinto.Terreno[temp[0], temp[1], temp[2]].RestrMovsInverso[2] = false;
                }
                else if (restMov[i] == "B")
                {
                    laberinto.Terreno[temp[0], temp[1], temp[2]].RestrMovsInverso[4] = false;
                }
                else if (restMov[i] == "U")
                {
                    laberinto.Terreno[temp[0], temp[1], temp[2]].RestrMovsInverso[5] = false;
                }
                else if (restMov[i] == "D")
                {
                    laberinto.Terreno[temp[0], temp[1], temp[2]].RestrMovsInverso[0] = false;
                }
            }
            #endregion
            //int numRobots = numRobotsBack + numRobotsDown + numRobotsFront + numRobotsLeft + numRobotsRight + numRobotsUp;
            //Console.WriteLine("Terminó de crear al juego y encontró "+ numRobots+" en el ultimo archivo de juego");
            
        }

        public bool ResolverPathFinds()
        {
            bool pathUp = false;
            bool pathDown = false;
            bool pathLeft = false;
            bool pathRight = false;
            bool pathFront = false;
            bool pathBack = false;
            if (this.laberinto.GoalUp != null)
            {
                //Console.WriteLine("Ingresando a PaathFindUp");
                pathUp = PathFindUp();
            }
            if(laberinto.GoalDown != null)
            {
                //Console.WriteLine("Ingresando a PaathFindDown");
                pathDown = PathFindDown();
            }
            //Console.WriteLine("Ingresando a PaathFindLeft");
            pathLeft = PathFindLeft();
            //Console.WriteLine("Ingresando a PaathFindRight");
            pathRight = PathFindRight();
            //Console.WriteLine("Ingresando a PaathFindFront");
            pathFront = PathFindFront();
            //Console.WriteLine("Ingresando a PaathFindBack");
            pathBack = PathFindBack();
            //Console.WriteLine("Comenzando a elegir rutas y pintado");
            //if (pathBack && pathFront && pathLeft && pathRight)
            {
                foreach (Robot rob in robotsList)
                {
                    rob.ElegirRuta(this.laberinto);
                    rob.Pintar(this.laberinto, this.imagenesLaberinto);
                }
            }
            
            return true;
        }
        public void PromediarColores()
        {
            double auxR;
            double auxG;
            double auxB;
            for (int z = 0; z < this.Z_Alto; z++)
            {
                for (int x = 0; x < this.X_Ancho; x++)
                {
                    for (int y = 0; y < this.Y_Profundidad; y++)
                    {
                        auxR = 0;
                        auxG = 0;
                        auxB = 0;
                        if (this.laberinto.Terreno[x, y, z].NumColores > 0)
                        {
                            auxR = this.laberinto.Terreno[x, y, z].SumColores[0] / this.laberinto.Terreno[x, y, z].NumColores;//ColorFinal[0];
                            auxG = this.laberinto.Terreno[x, y, z].SumColores[1] / this.laberinto.Terreno[x, y, z].NumColores;//this.laberinto.Terreno[x, y, z].ColorFinal[1];
                            auxB = this.laberinto.Terreno[x, y, z].SumColores[2] / this.laberinto.Terreno[x, y, z].NumColores;//this.laberinto.Terreno[x, y, z].ColorFinal[2];
                            auxR = Math.Truncate(auxR);
                            auxG = Math.Truncate(auxG );
                            auxB = Math.Truncate(auxB );
                            if (auxR < 0) { auxR = 0; }
                            else if (auxR > 255) { auxR = 255; }
                            if (auxG < 0) { auxG = 0; }
                            else if (auxG > 255) { auxG = 255; }
                            if (auxB < 0) { auxB = 0; }
                            else if (auxB > 255) { auxB = 255; }

                        }
                        this.laberinto.Terreno[x, y, z].ColorFinal[0] = (int)auxR;
                        this.laberinto.Terreno[x, y, z].ColorFinal[1] = (int)auxG;
                        this.laberinto.Terreno[x, y, z].ColorFinal[2] = (int)auxB;

                    }
                }
            }
            /*double auxR;
            double auxG;
            double auxB;
            for (int z = 0;z<this.Z_Alto;z++)
            {
                for (int x = 0; x < this.X_Ancho; x++)
                {
                    for (int y=0;y<this.Y_Profundidad;y++)
                    {
                        auxR = this.laberinto.Terreno[x, y, z].ColorFinal[0];
                        auxG = this.laberinto.Terreno[x, y, z].ColorFinal[1];
                        auxB = this.laberinto.Terreno[x, y, z].ColorFinal[2];
                        if (this.laberinto.Terreno[x, y, z].ColoresRespuesta.Count > 0)
                        {
                            foreach (int[] col in this.laberinto.Terreno[x, y, z].ColoresRespuesta)
                            {
                                auxR += col[0];
                                auxG += col[1];
                                auxB += col[2];
                            }
                            auxR = Math.Truncate(auxR / this.laberinto.Terreno[x, y, z].ColoresRespuesta.Count);
                            auxG = Math.Truncate(auxG / this.laberinto.Terreno[x, y, z].ColoresRespuesta.Count);
                            auxB = Math.Truncate(auxB / this.laberinto.Terreno[x, y, z].ColoresRespuesta.Count);
                            
                            if (auxR < 0) { auxR = 0; }
                            else if (auxR >255 ) { auxR = 255; }
                            if (auxG < 0) { auxG = 0; }
                            else if (auxG > 255) { auxG = 255; }
                            if (auxB < 0) { auxB = 0; }
                            else if (auxB > 255) { auxB = 255; }
                            
                        }
                        this.laberinto.Terreno[x, y, z].ColorFinal[0] = (int)auxR;
                        this.laberinto.Terreno[x, y, z].ColorFinal[1] = (int)auxG;
                        this.laberinto.Terreno[x, y, z].ColorFinal[2] = (int)auxB;

                    }
                }
            }*/
        }

        public List<Bitmap> SetearImagenesOutput(int tag )
        {
            
            List<Bitmap> imagenesOutPut = new List<Bitmap>();
            for (int z = 0; z < this.Z_Alto; z++)
            {
                Bitmap bmp = new Bitmap(X_Ancho, Y_Profundidad, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
                for (int x = 0; x < this.X_Ancho; x++)
                {
                    for (int y = 0; y < Y_Profundidad; y++)
                    {
                        bmp.SetPixel(x, y, Color.FromArgb(
                            this.laberinto.Terreno[x, y, z].ColorFinal[0], this.laberinto.Terreno[x, y, z].ColorFinal[1], this.laberinto.Terreno[x, y, z].ColorFinal[2]));
                    }
                }
                bmp.Tag = tag;
                tag++;
                //Console.WriteLine("Cargando imagen "+z+" a lista imageneOutPut");
                imagenesOutPut.Add(bmp);
            }
            return imagenesOutPut;
            
        }

        public bool PathFindUp()//Laberinto laberinto)
        {
            Celda goal = this.laberinto.GoalUp;
            //Desexplorar();//Hago por defecto todo desexplorado
            bool succesUp = false;
            // Listas en las que se guardan los próximos nodos a explorar
            List<Celda> openListUp = new List<Celda>();
            List<Celda> openListUp2 = new List<Celda>();
            // El primer nodo a agregar es el objetivo
            openListUp.Add(goal);
            goal.CostoUp = 0;
            goal.ExploradoUp = true;
            this.numRobotsAlcanzadosUp = 0;
            // mientras queden elementos en la lista continuar procesando
            while (openListUp.Count != 0 && !succesUp)//isEmpty())
            {
                // Para cada movimiento en el orden de preferencia
                for (int k = 0; k < movsFactiblesInverso.Count; k++)//foreach (int[] movInverso in movsFactiblesInverso)
                {
                    int[] movInverso = movsFactiblesInverso[k];
                    //Reviso para cada celda solo en este movimiento si no ha sido explorada y si me puedo mover, y actualizo predecesor, costo y conteo de robots alcanzados y la hago explorada
                    foreach (Celda n in openListUp)
                    {
                        if (
                            ((n.X + movInverso[0]) < X_Ancho && (n.Y + movInverso[1]) < Y_Profundidad && (n.Z + movInverso[2]) < Z_Alto) &&
                            ((n.X + movInverso[0]) >= 0 &&(n.Y + movInverso[1]) >= 0 &&(n.Z + movInverso[2]) >= 0))
                        {
                            if (n.RestrMovsInverso[k])
                            {
                                if (this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoUp)
                                { continue; }
                                this.laberinto.Terreno[n.X + movInverso[0],n.Y +movInverso[1],n.Z + movInverso[2]].ExploradoUp=true;
                                this.laberinto.Terreno[n.X + movInverso[0],n.Y +movInverso[1],n.Z + movInverso[2]].CostoUp = n.CostoUp + 1;
                                this.laberinto.Terreno[n.X + movInverso[0],n.Y +movInverso[1],n.Z + movInverso[2]].PredecesorUp = n;
                                if(n.RobotInHere && n.RobotCelda.OutUp)
                                {
                                    numRobotsAlcanzadosUp++;
                                    //numRobotsTotal++;

                                    if (numRobotsAlcanzadosUp == numRobotsUp)//En rigor sería más eficiente parar cuando llega a numrobotsAlcanzadosUp, pero para
                                        //eso tendría que preguntar si el robot en la celda tiene OutUp true además de si n.robotInHere.  Aun no se me ocurre como sin 
                                        //asignarle un robot a cada celda, que sería algo circular.  Al menos es una 2da mejor antes que terminar todo el laberinto.
                                        //Seteado:  Celda conoce el robot que la habita, el robot no conoce la celda ni al laberinto ni a juego
                                    {
                                        Console.WriteLine("PathFindUp terminó exitosamente.");
                                        succesUp = true;
                                        return succesUp;
                                    }
                                }
                                openListUp2.Add(this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]]);
                            }
                            else {  continue;}
                        }
                        else {  continue;}
                    }
                }
                // se intercambian las listas
                openListUp.Clear();
                openListUp.AddRange(openListUp2);
                openListUp2.Clear();
            }
            Console.WriteLine("No se acabó el PathFindUp");
            return succesUp;
        }

        public bool PathFindDown()
        {
            Celda goal = this.laberinto.GoalDown;
            //Desexplorar();//Hago por defecto todo desexplorado
            bool succesDown = false;
            // Listas en las que se guardan los próximos nodos a explorar
            List<Celda> openListDown = new List<Celda>();
            List<Celda> openListDown2 = new List<Celda>();
            // El primer nodo a agregar es el objetivo
            openListDown.Add(goal);
            goal.CostoDown = 0;
            goal.ExploradoDown = true;
            this.numRobotsAlcanzadosDown = 0;
            // mientras queden elementos en la lista continuar procesando
            while (openListDown.Count != 0 && !succesDown)//isEmpty())
            {
                // Para cada movimiento en el orden de preferencia
                for (int k = 0; k < movsFactiblesInverso.Count; k++)//foreach (int[] movInverso in movsFactiblesInverso)
                {
                    int[] movInverso = movsFactiblesInverso[k];
                    //Reviso para cada celda solo en este movimiento si no ha sido explorada y si me puedo mover, y actualizo predecesor, costo y conteo de robots alcanzados y la hago explorada
                    foreach (Celda n in openListDown)
                    {
                        if (((n.X + movInverso[0]) < X_Ancho && (n.Y + movInverso[1]) < Y_Profundidad && (n.Z + movInverso[2]) < Z_Alto) &&
                            ((n.X + movInverso[0]) >= 0 && (n.Y + movInverso[1]) >= 0 && (n.Z + movInverso[2]) >= 0))
                        {
                            if (n.RestrMovsInverso[k])
                            {
                                if (this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoDown)
                                { continue; }
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoDown = true;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].CostoDown = n.CostoDown + 1;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].PredecesorDown = n;
                                if (n.RobotInHere && n.RobotCelda.OutDown)
                                {
                                    numRobotsAlcanzadosDown++;
                                    //numRobotsTotal++;
                                    if (numRobotsAlcanzadosDown == numRobotsDown)
                                    {
                                        Console.WriteLine("PathFindDown termino con éxito.");
                                        succesDown = true;
                                        return succesDown;
                                    }
                                }
                                openListDown2.Add(this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]]);
                            }
                            else { continue; }
                        }
                        else { continue; }
                    }
                }
                // se intercambian las listas
                openListDown.Clear();
                openListDown.AddRange(openListDown2);
                openListDown2.Clear();
            }
            Console.WriteLine("No se acabó el PathFindDown");
            return succesDown;
        }

        public bool PathFindLeft()
        {
            Celda goal = this.laberinto.GoalLeft;
            //Desexplorar();//Hago por defecto todo desexplorado
            bool succesLeft = false;
            // Listas en las que se guardan los próximos nodos a explorar
            List<Celda> openListLeft = new List<Celda>();
            List<Celda> openListLeft2 = new List<Celda>();
            // El primer nodo a agregar es el objetivo
            openListLeft.Add(goal);
            goal.CostoLeft = 0;
            goal.ExploradoLeft = true;
            this.numRobotsAlcanzadosLeft = 0;
            // mientras queden elementos en la lista continuar procesando
            while (openListLeft.Count != 0 )//isEmpty())
            {
                // Para cada movimiento en el orden de preferencia
                for (int k=0;k<movsFactiblesInverso.Count;k++)//int[] movInverso in movsFactiblesInverso)
                {
                    int[] movInverso = movsFactiblesInverso[k];
                    //Reviso para cada celda solo en este movimiento si no ha sido explorada y si me puedo mover, y actualizo predecesor, costo y conteo de robots alcanzados y la hago explorada
                    foreach (Celda n in openListLeft)
                    {
                        
                        if (
                            ((n.X +  movInverso[0]) < X_Ancho && (n.Y + movInverso[1]) < Y_Profundidad && (n.Z + movInverso[2]) < Z_Alto) &&
                            ((n.X + movInverso[0]) >= 0 && (n.Y + movInverso[1]) >= 0 && (n.Z + movInverso[2]) >= 0))
                        {
                            if (n.RestrMovsInverso[k])
                            {
                                if (!this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoLeft)
                                { continue; }
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoLeft = true;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].CostoLeft = n.CostoLeft + 1;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].PredecesorLeft = n;
                                if (n.RobotInHere && n.RobotCelda.OutLeft)
                                {
                                    numRobotsAlcanzadosLeft++;
                                    if (numRobotsAlcanzadosLeft == numRobotsLeft)
                                    {
                                        Console.WriteLine("PathFindLeft realizado con éxito");
                                        succesLeft = true;
                                        //return succesLeft;
                                    }
                                }
                                openListLeft2.Add(this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]]);
                            }
                            //else { continue; }
                        }
                        //else { continue; }
                    }
                }
                // se intercambian las listas
                openListLeft.Clear();
                openListLeft.AddRange(openListLeft2);
                openListLeft2.Clear();
            }
            //Console.WriteLine("No se acabó el PathFindLeft");
            return succesLeft;
        }

        public bool PathFindRight()
        {
            Celda goal = this.laberinto.GoalRight;
            //Desexplorar();//Hago por defecto todo desexplorado
            bool succesRight = false;
            // Listas en las que se guardan los próximos nodos a explorar
            List<Celda> openListRight = new List<Celda>();
            List<Celda> openListRight2 = new List<Celda>();
            // El primer nodo a agregar es el objetivo
            openListRight.Add(goal);
            goal.CostoRight = 0;
            goal.ExploradoRight = true;
            numRobotsAlcanzadosRight = 0;
            // mientras queden elementos en la lista continuar procesando
            while (openListRight.Count != 0 && !succesRight)//isEmpty())
            {
                // Para cada movimiento en el orden de preferencia
                for (int k = 0; k < movsFactiblesInverso.Count; k++)//foreach (int[] movInverso in movsFactiblesInverso)
                {
                    int[] movInverso = movsFactiblesInverso[k];
                    //Reviso para cada celda solo en este movimiento si no ha sido explorada y si me puedo mover, y actualizo predecesor, costo y conteo de robots alcanzados y la hago explorada
                    foreach (Celda n in openListRight)
                    {
                        if (
                            ((n.X + movInverso[0]) < X_Ancho && (n.Y + movInverso[1]) < Y_Profundidad && (n.Z + movInverso[2]) < Z_Alto) &&
                            ((n.X + movInverso[0]) >= 0 && (n.Y + movInverso[1]) >= 0 && (n.Z + movInverso[2]) >= 0))
                        {
                            if (n.RestrMovsInverso[k])
                            {
                                if (this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoRight)
                                { continue; }
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoRight = true;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].CostoRight = n.CostoRight + 1;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].PredecesorRight = n;
                                if (n.RobotInHere && n.RobotCelda.OutRight)
                                {
                                    numRobotsAlcanzadosRight++;
                                    if (numRobotsAlcanzadosRight == numRobotsRight)
                                    {

                                        Console.WriteLine("PathFindRight terminado con éxito");
                                        succesRight = true;
                                        return succesRight;
                                    }
                                }
                                openListRight2.Add(this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]]);
                            }
                            else { continue; }
                        }
                        else { continue; }
                    }
                }
                // se intercambian las listas
                openListRight.Clear();
                openListRight.AddRange(openListRight2);
                openListRight2.Clear();
            }
            Console.WriteLine("No se acabó el PathFindRight");
            return succesRight;
        }

        public bool PathFindFront()
        {
            Celda goal = this.laberinto.GoalFront;
            //Desexplorar();//Hago por defecto todo desexplorado
            bool succesFront = false;
            // Listas en las que se guardan los próximos nodos a explorar
            List<Celda> openListFront = new List<Celda>();
            List<Celda> openListFront2 = new List<Celda>();
            // El primer nodo a agregar es el objetivo
            openListFront.Add(goal);
            goal.CostoFront = 0;
            goal.ExploradoFront = true;
            numRobotsAlcanzadosFront = 0;
            // mientras queden elementos en la lista continuar procesando
            while (openListFront.Count != 0)//isEmpty())
            {
                // Para cada movimiento en el orden de preferencia
                for (int k = 0; k < movsFactiblesInverso.Count; k++)//foreach (int[] movInverso in movsFactiblesInverso)
                {
                    int[] movInverso = movsFactiblesInverso[k];
                    //Reviso para cada celda solo en este movimiento si no ha sido explorada y si me puedo mover, y actualizo predecesor, costo y conteo de robots alcanzados y la hago explorada
                    foreach (Celda n in openListFront)
                    {
                        if (
                            ((n.X + movInverso[0]) < X_Ancho && (n.Y + movInverso[1]) < Y_Profundidad && (n.Z + movInverso[2]) < Z_Alto) &&
                            ((n.X + movInverso[0]) >= 0 && (n.Y + movInverso[1]) >= 0 && (n.Z + movInverso[2]) >= 0))
                        {
                            if (n.RestrMovsInverso[k])
                            {
                                if (this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoFront)
                                { continue; }
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoFront = true;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].CostoFront = n.CostoFront + 1;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].PredecesorFront = n;
                                if (n.RobotInHere && n.RobotCelda.OutFront)
                                {
                                    numRobotsAlcanzadosFront++;
                                    if (numRobotsAlcanzadosFront == numRobotsFront)
                                    {
                                        Console.WriteLine("PathFindFront terminado con éxito.");
                                        succesFront = true;
                                        return succesFront;
                                    }
                                }
                                openListFront2.Add(this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]]);
                            }
                            else { continue; }
                        }
                        else { continue; }
                    }
                }
                // se intercambian las listas
                openListFront.Clear();
                openListFront.AddRange(openListFront2);
                openListFront2.Clear();
            }
            Console.WriteLine("No se acabó el PathFindFront");
            return succesFront;
        }

        public bool PathFindBack()
        {
            Celda goal = this.laberinto.GoalBack;
            //Desexplorar();//Hago por defecto todo desexplorado
            bool succesBack = false;
            // Listas en las que se guardan los próximos nodos a explorar
            List<Celda> openListBack = new List<Celda>();
            List<Celda> openListBack2 = new List<Celda>();
            // El primer nodo a agregar es el objetivo
            openListBack.Add(goal);
            goal.CostoBack = 0;
            goal.ExploradoBack = true;
            numRobotsAlcanzadosBack = 0;
            // mientras queden elementos en la lista continuar procesando
            while (openListBack.Count != 0 )//isEmpty())
            {
                // Para cada movimiento en el orden de preferencia
                for (int k = 0; k < movsFactiblesInverso.Count; k++)//foreach (int[] movInverso in movsFactiblesInverso)
                {
                    int[] movInverso = movsFactiblesInverso[k];
                    //Reviso para cada celda solo en este movimiento si no ha sido explorada y si me puedo mover, y actualizo predecesor, costo y conteo de robots alcanzados y la hago explorada
                    foreach (Celda n in openListBack)
                    {
                        if (
                            ((n.X + movInverso[0]) < X_Ancho && (n.Y + movInverso[1]) < Y_Profundidad && (n.Z + movInverso[2]) < Z_Alto) &&
                            ((n.X + movInverso[0]) >= 0 && (n.Y + movInverso[1]) >= 0 && (n.Z + movInverso[2]) >= 0))
                        {
                            if (n.RestrMovsInverso[k])
                            {
                                if (this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoBack)
                                { continue; }
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].ExploradoBack = true;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].CostoBack = n.CostoBack + 1;
                                this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]].PredecesorBack = n;
                                if (n.RobotInHere && n.RobotCelda.OutBack)
                                {
                                    numRobotsAlcanzadosBack++;
                                    if (numRobotsAlcanzadosBack == numRobotsBack)
                                    {
                                        Console.WriteLine("PathFindBack terminó exitosamente");
                                        succesBack = true;
                                        return succesBack;
                                    }
                                }
                                openListBack2.Add(this.laberinto.Terreno[n.X + movInverso[0], n.Y + movInverso[1], n.Z + movInverso[2]]);
                            }
                            //else { continue; }
                        }
                        //else { continue; }
                    }
                }
                // se intercambian las listas
                openListBack.Clear();
                openListBack.AddRange(openListBack2);
                openListBack2.Clear();
            }
            //Console.WriteLine("No se acabó el PathFindBack");
            return succesBack;
        }

        public void Desexplorar() //Este metodo ya no es necesario pues el constructor de celda las hace ya desexploradas
        {
            /*
            // Hacer ninguno explorado
            for (int i = 0; i < X_Ancho; ++i)
                for (int j = 0; j < Y_Profundidad; ++j)
                    for (int k = 0; k < Z_Alto;k++ )
                        this.laberinto.Terreno[i, j,k].Explorado = false;
             * */
        }
    }
     
}
